float Incremental_PID(float setpoint, float process_var) {
    static float err_last = 0;     // 上次误差
    static float err_2last = 0;    // 上上次误差
    static float output = 0;        // 当前输出值
    
    float error = setpoint - process_var;
    // 计算控制增量
    float delta_u = Kp * (error - err_last) 
                  + Ki * error 
                  + Kd * (error - 2*err_last + err_2last);
    
    output += delta_u;  // 更新输出
    // 输出限幅（如PWM占空比0~100%）
    if (output > 100.0f) output = 100.0f;
    if (output < 0.0f) output = 0.0f;
    
    // 更新误差记录
    err_2last = err_last;
    err_last = error;
    return output;
}